package es.uji.viselab.robot;

public class M3 {

	public static double[][] rotY(double[][] m3, double alfa) {
		double[][] rot = new double[3][3];
		rot = M3.getRotY(alfa);
		return M3.mult(m3, rot);

	}

	static double[][] mult(double[][] a, double[][] b) {
		double[][] r = new double[3][3];
		for (int i = 0; i < 3; i++)
			for (int j = 0; j < 3; j++) {
				r[i][j] = 0;
				for (int k = 0; k < 3; k++)
					r[i][j] = r[i][j] + a[i][k] * b[k][j];
			}
		return r;
	}

	private static double[][] getZero() {
		double[][] r = new double[3][3];
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				r[i][j] = 0;
			}
		}
		return r;
	}

	private static double[][] getRotY(double a) {
		double[][] rot = new double[3][3];
		rot[0][0] = Math.cos(a);
		rot[0][1] = 0;
		rot[0][2] = Math.sin(a);

		rot[1][0] = 0;
		rot[1][1] = 1;
		rot[1][2] = 0;

		rot[2][0] = -1 * Math.sin(a);
		rot[2][1] = 0;
		rot[2][2] = Math.cos(a);

		return rot;
	}

}
